#include "zf_common_headfile.h"
#include <math.h>
/*
int16 imu963ra_gyro_x = 0, imu963ra_gyro_y = 0, imu963ra_gyro_z = 0;       // 三轴陀螺仪数据      GYRO (陀螺仪)
int16 imu963ra_acc_x = 0,  imu963ra_acc_y = 0,  imu963ra_acc_z = 0;        // 三轴加速度计数据     ACC  (accelerometer 加速度计)
int16 imu963ra_mag_x = 0,  imu963ra_mag_y = 0,  imu963ra_mag_z = 0;        // 三轴磁力计数据      MAG  (magnetometer 磁力计)
 */
static float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
static float exInt = 0, eyInt = 0, ezInt = 0;
static short turns=0;
static float newdata=0.0f,olddata=0.0f;
static float pitchoffset,rolloffset,yawoffset;

static float k10=0.0f,k11=0.0f,k12=0.0f,k13=0.0f;
static float k20=0.0f,k21=0.0f,k22=0.0f,k23=0.0f;
static float k30=0.0f,k31=0.0f,k32=0.0f,k33=0.0f;
static float k40=0.0f,k41=0.0f,k42=0.0f,k43=0.0f;

float pitch,roll,yaw;
float gx,gy,gz;
float ax,ay,az;
float mx,my,mz;

short gyro_offsetx=0,gyro_offsety=0,gyro_offsetz=0;
float tmp1,tmp2,tmp3;
float magoffsetx=1.31454428611172,magoffsety=-1.21753632395713,magoffsetz=1.6567777185719;
float B[6]={0.980358187761106,-0.0105514731414606,0.00754899338354401,0.950648704823113,-0.0354995317649016,1.07449478456729};
float accoffsetx=-0.0118443148713821,accoffsety=-0.00939616830387326,accoffsetz=-0.0507891438815726;
float accsensx=1.00851297697413,accsensy=0.991366909333871,accsensz=1.00019364448499;

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     融合加速度计和磁力计进行姿态调整
// 参数说明     无
// 返回参数     无
// 使用示例     AHRSupdate(gx,gy,gz,ax,ay,az,mx,my,mz,&roll,&pitch,&yaw);
// 备注信息     内部调用
//-------------------------------------------------------------------------------------------------------------------
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz,float *roll,float *pitch,float *yaw)
{
           float norm;                                  //用于单位化
           float hx, hy, hz, bx, bz;        //
           float vx, vy, vz, wx, wy, wz;
           float ex, ey, ez;
//                   float tmp0,tmp1,tmp2,tmp3;

           // auxiliary variables to reduce number of repeated operations  辅助变量减少重复操作次数
           float q0q0 = q0*q0;
           float q0q1 = q0*q1;
           float q0q2 = q0*q2;
           float q0q3 = q0*q3;
           float q1q1 = q1*q1;
           float q1q2 = q1*q2;
           float q1q3 = q1*q3;
           float q2q2 = q2*q2;
           float q2q3 = q2*q3;
           float q3q3 = q3*q3;

           // normalise the measurements  对加速度计和磁力计数据进行规范化
           norm = invSqrt(ax*ax + ay*ay + az*az);
           ax = ax * norm;
           ay = ay * norm;
           az = az * norm;
           norm = invSqrt(mx*mx + my*my + mz*mz);
           mx = mx * norm;
           my = my * norm;
           mz = mz * norm;

           // compute reference direction of magnetic field  计算磁场的参考方向
                     //hx,hy,hz是mx,my,mz在参考坐标系的表示
           hx = 2*mx*(0.50 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
           hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.50 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
           hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.50 - q1q1 -q2q2);
                        //bx,by,bz是地球磁场在参考坐标系的表示
           bx = sqrt((hx*hx) + (hy*hy));
           bz = hz;

// estimated direction of gravity and magnetic field (v and w)  //估计重力和磁场的方向
//vx,vy,vz是重力加速度在物体坐标系的表示
           vx = 2*(q1q3 - q0q2);
           vy = 2*(q0q1 + q2q3);
           vz = q0q0 - q1q1 - q2q2 + q3q3;
                     //wx,wy,wz是地磁场在物体坐标系的表示
           wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
           wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
           wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);

// error is sum ofcross product between reference direction of fields and directionmeasured by sensors
//ex,ey,ez是加速度计与磁力计测量出的方向与实际重力加速度与地磁场方向的误差，误差用叉积来表示，且加速度计与磁力计的权重是一样的
           ex = (ay*vz - az*vy) + (my*wz - mz*wy);
           ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
           ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

           // integral error scaled integral gain
                     //积分误差
           exInt = exInt + ex*Ki*dt;
           eyInt = eyInt + ey*Ki*dt;
           ezInt = ezInt + ez*Ki*dt;
                    // printf("exInt=%0.1f eyInt=%0.1f ezInt=%0.1f ",exInt,eyInt,ezInt);
           // adjusted gyroscope measurements
                     //PI调节陀螺仪数据
           gx = gx + Kp*ex + exInt;
           gy = gy + Kp*ey + eyInt;
           gz = gz + Kp*ez + ezInt;
                     //printf("gx=%0.1f gy=%0.1f gz=%0.1f",gx,gy,gz);

           // integrate quaernion rate aafnd normalaizle
                     //欧拉法解微分方程
//           tmp0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
//           tmp1 = q1 + ( q0*gx + q2*gz - q3*gy)*halfT;
//           tmp2 = q2 + ( q0*gy - q1*gz + q3*gx)*halfT;
//           tmp3 = q3 + ( q0*gz + q1*gy - q2*gx)*halfT;
//                   q0=tmp0;
//                   q1=tmp1;
//                   q2=tmp2;
//                   q3=tmp3;
                     //printf("q0=%0.1f q1=%0.1f q2=%0.1f q3=%0.1f",q0,q1,q2,q3);
////RUNGE_KUTTA 法解微分方程
           k10=0.5 * (-gx*q1 - gy*q2 - gz*q3);
           k11=0.5 * ( gx*q0 + gz*q2 - gy*q3);
           k12=0.5 * ( gy*q0 - gz*q1 + gx*q3);
           k13=0.5 * ( gz*q0 + gy*q1 - gx*q2);

           k20=0.5 * (halfT*(q0+halfT*k10) + (halfT-gx)*(q1+halfT*k11) + (halfT-gy)*(q2+halfT*k12) + (halfT-gz)*(q3+halfT*k13));
           k21=0.5 * ((halfT+gx)*(q0+halfT*k10) + halfT*(q1+halfT*k11) + (halfT+gz)*(q2+halfT*k12) + (halfT-gy)*(q3+halfT*k13));
           k22=0.5 * ((halfT+gy)*(q0+halfT*k10) + (halfT-gz)*(q1+halfT*k11) + halfT*(q2+halfT*k12) + (halfT+gx)*(q3+halfT*k13));
           k23=0.5 * ((halfT+gz)*(q0+halfT*k10) + (halfT+gy)*(q1+halfT*k11) + (halfT-gx)*(q2+halfT*k12) + halfT*(q3+halfT*k13));

           k30=0.5 * (halfT*(q0+halfT*k20) + (halfT-gx)*(q1+halfT*k21) + (halfT-gy)*(q2+halfT*k22) + (halfT-gz)*(q3+halfT*k23));
           k31=0.5 * ((halfT+gx)*(q0+halfT*k20) + halfT*(q1+halfT*k21) + (halfT+gz)*(q2+halfT*k22) + (halfT-gy)*(q3+halfT*k23));
           k32=0.5 * ((halfT+gy)*(q0+halfT*k20) + (halfT-gz)*(q1+halfT*k21) + halfT*(q2+halfT*k22) + (halfT+gx)*(q3+halfT*k23));
           k33=0.5 * ((halfT+gz)*(q0+halfT*k20) + (halfT+gy)*(q1+halfT*k21) + (halfT-gx)*(q2+halfT*k22) + halfT*(q3+halfT*k23));

           k40=0.5 * (dt*(q0+dt*k30) + (dt-gx)*(q1+dt*k31) + (dt-gy)*(q2+dt*k32) + (dt-gz)*(q3+dt*k33));
           k41=0.5 * ((dt+gx)*(q0+dt*k30) + dt*(q1+dt*k31) + (dt+gz)*(q2+dt*k32) + (dt-gy)*(q3+dt*k33));
           k42=0.5 * ((dt+gy)*(q0+dt*k30) + (dt-gz)*(q1+dt*k31) + dt*(q2+dt*k32) + (dt+gx)*(q3+dt*k33));
           k43=0.5 * ((dt+gz)*(q0+dt*k30) + (dt+gy)*(q1+dt*k31) + (dt-gx)*(q2+dt*k32) + dt*(q3+dt*k33));

           q0=q0 + dt/6.0 * (k10+2*k20+2*k30+k40);
           q1=q1 + dt/6.0 * (k11+2*k21+2*k31+k41);
           q2=q2 + dt/6.0 * (k12+2*k22+2*k32+k42);
           q3=q3 + dt/6.0 * (k13+2*k23+2*k33+k43);

           // normalise quaternion
           norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
           q0 = q0 * norm;
           q1 = q1 * norm;
           q2 = q2 * norm;
           q3 = q3 * norm;

           *pitch = asin(-2 * q1 * q3 + 2 * q0 * q2)* 57.3;   // pitch
           *roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1)* 57.3;   // roll
           *yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;  //yaw
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     计算水平方向转的圈数
// 参数说明     无
// 返回参数     无
// 使用示例     CountTurns(&newdata,&olddata,&turns);
// 备注信息     内部调用
//-------------------------------------------------------------------------------------------------------------------
void CountTurns(float *newdata,float *olddata,short *turns)
{
    if (*newdata<-170.0f && *olddata>170.0f)
        (*turns)++;
    if (*newdata>170.0f && *olddata<-170.0f)
        (*turns)--;

}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     计算偏航角
// 参数说明     无
// 返回参数     无
// 使用示例     CalYaw(&yaw,&turns);
// 备注信息     内部调用
//-------------------------------------------------------------------------------------------------------------------
void CalYaw(float *yaw,short *turns)
{
    *yaw=360.0**turns+*yaw;
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     补偿欧拉角偏移，主要补偿yaw角
// 参数说明     无
// 返回参数     无
// 使用示例     MPU_Get_Gyro(&imu963ra_gyro_x,&imu963ra_gyro_y,&imu963ra_gyro_z,&gx,&gy,&gz);
// 备注信息     内部调用
//-------------------------------------------------------------------------------------------------------------------
void MPU_Get_Gyro(int16 *igx,int16 *igy,int16 *igz,float *gx,float *gy,float *gz)
{

    *gx=(float)(*igx)*gryo_scale;
    *gy=(float)(*igy)*gryo_scale;
    *gz=(float)(*igz)*gryo_scale;

}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     补偿欧拉角偏移，主要补偿yaw角
// 参数说明     无
// 返回参数     无
// 使用示例     MPU_Get_Accel(&imu963ra_acc_x,&imu963ra_acc_y,&imu963ra_acc_z,&ax,&ay,&az);
// 备注信息     内部调用
//-------------------------------------------------------------------------------------------------------------------
void MPU_Get_Accel(int16 *iax,int16 *iay,int16 *iaz,float *ax,float *ay,float *az)
{

    tmp1=(float)(*iax)*accel_scale-accoffsetx;
    tmp2=(float)(*iay)*accel_scale-accoffsety;
    tmp3=(float)(*iaz)*accel_scale-accoffsetz;
    *ax=tmp1*accsensx;
    *ay=tmp2*accsensy;
    *az=tmp3*accsensz;

}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     获得磁力计数据，单位高斯，并对磁力计进行补偿
// 参数说明     无
// 返回参数     无
// 使用示例     MPU_Get_Mag(&imu963ra_mag_x,&imu963ra_mag_y,&imu963ra_mag_z,&mx,&my,&mz);
// 备注信息     内部调用
//-------------------------------------------------------------------------------------------------------------------
void MPU_Get_Mag(int16 *imx,int16 *imy,int16 *imz,float *mx,float *my,float *mz)
{
    tmp1=(float)(*imx)*mag_scale-magoffsetx;
    tmp2=(float)(*imy)*mag_scale-magoffsety;
    tmp3=(float)(*imz)*mag_scale-magoffsetz;
    *mx=B[0]*tmp1+B[1]*tmp2+B[2]*tmp3;
    *my=B[1]*tmp1+B[3]*tmp2+B[4]*tmp3;
    *mz=B[2]*tmp1+B[4]*tmp2+B[5]*tmp3;
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     获取原始值
// 参数说明     无
// 返回参数     无
// 使用示例     Imu_get_all();
// 备注信息     内部调用
//-------------------------------------------------------------------------------------------------------------------
void Imu_get_all(void)
{
    imu963ra_get_mag();
    imu963ra_get_gyro();
    imu963ra_get_acc();
    MPU_Get_Gyro(&imu963ra_gyro_x,&imu963ra_gyro_y,&imu963ra_gyro_z,&gx,&gy,&gz);
    MPU_Get_Accel(&imu963ra_acc_x,&imu963ra_acc_y,&imu963ra_acc_z,&ax,&ay,&az);
    MPU_Get_Mag(&imu963ra_mag_x,&imu963ra_mag_y,&imu963ra_mag_z,&mx,&my,&mz);
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     初始化并获得初始姿态角
// 参数说明     无
// 返回参数     无
// 使用示例     CalibrateToZero();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void CalibrateToZero(void)
{

    uint8 t=0;
    float sumpitch=0,sumroll=0,sumyaw=0;
    float pitch,roll,yaw;
    imu963ra_init ();
    for (t=0;t<150;t++)
 {
        Imu_get_all();
        AHRSupdate(gx,gy,gz,ax,ay,az,mx,my,mz,&roll,&pitch,&yaw);
        if (t>=100)
        {
            sumpitch+=pitch;
            sumroll+=roll;
            sumyaw+=yaw;
        }
 }
    pitchoffset=-sumpitch/50.0f;
    rolloffset=-sumroll/50.0f;
    yawoffset=-sumyaw/50.0f;
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     进行姿态解算
// 参数说明     无
// 返回参数     无
// 使用示例     ALL_Mpu_posture();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void ALL_Mpu_posture(void)
{
    Imu_get_all();
    AHRSupdate(gx,gy,gz,ax,ay,az,mx,my,mz,&roll,&pitch,&yaw);
    olddata=newdata;
    newdata=yaw;
    CountTurns(&newdata,&olddata,&turns);
    CalYaw(&yaw,&turns);
    pitch+=pitchoffset;
    roll+=rolloffset;
    yaw+=yawoffset;
}

